﻿/*****************************************************************************
** Includes
*****************************************************************************/

#include "RobotCmdNode.hpp"
#include "RobotServerCmd.h"

/*****************************************************************************
** Namespaces
*****************************************************************************/
using namespace std::chrono_literals;
using std::placeholders::_1;

/*****************************************************************************
** Implementation
*****************************************************************************/

RobotCmdNode::RobotCmdNode(RobotServerCmd* cmdServer)
    : Node("robotservercmd_node")
{
    m_pubCmd = this->create_publisher<pccmd_msg::msg::PcCmd>("PcCmdMsg", 10);
    m_pubClawCmd =
        this->create_publisher<clawcmd_msg::msg::ClawCmd>("ClawCmd", 10);
    m_pubParam = this->create_publisher<param_msg::msg::Param>("ParamMsg", 10);
    m_pubSerialTool = this->create_publisher<serialtool_msg::msg::SerialTool>("SerialTool", 1);

    m_subJsonBa = this->create_subscription<jsonba_msg::msg::JsonBa>(
        "JsonBa", 10, std::bind(&RobotCmdNode::jsonba_callback, this, _1));

    m_subMapText = this->create_subscription<maptext_msg::msg::MapText>(
        "MapText", 10, std::bind(&RobotCmdNode::maptext_callback, this, _1));

    m_cmdServer = cmdServer;
}

RobotCmdNode::~RobotCmdNode()
{
    rclcpp::shutdown();
}

void RobotCmdNode::sendPcCmd(const std::string& cmdname,
                             const std::string& cmdcontent)
{
    auto cmd_msg = pccmd_msg::msg::PcCmd();
    cmd_msg.cmdname = cmdname;
    cmd_msg.cmdcontent = cmdcontent;
    m_pubCmd->publish(cmd_msg);

    RCLCPP_INFO(this->get_logger(),
                "RobotCmdNode sendPcCmd, cmdname: %s, cmdcontent: %s",
                cmdname.c_str(), cmdcontent.c_str());
}

void RobotCmdNode::clawControl(uint8_t claw_dir, uint8_t claw_force)
{
    static int s_lastForce = 0;
    s_lastForce = claw_force;
    auto cmd_msg = serialtool_msg::msg::SerialTool();
    cmd_msg.cmdname = "serialtools";
    if (claw_force == 2) {
        cmd_msg.cmdcontent = "jq_stop2song";
    } else if (claw_force == 1) {
        cmd_msg.cmdcontent = "jq_stop2jin";
    } else if (claw_force == 3) {
        if (s_lastForce == 1) {
            cmd_msg.cmdcontent = "jq_jin2stop";
        } else if (s_lastForce == 2) {
            cmd_msg.cmdcontent = "jq_song2stop";
        }
    }
    m_pubSerialTool->publish(cmd_msg);

    RCLCPP_INFO(this->get_logger(), "robotserver_cmd RobotCmdNode claw control, claw dir: %d, claw force: %d", claw_dir, claw_force);
}

void RobotCmdNode::sendParamMsg(const std::string& cmdname,
                                const std::vector<std::string>& params)
{
    auto paramMsg = param_msg::msg::Param();
    paramMsg.cmdname = cmdname;
    paramMsg.params = params;

    m_pubParam->publish(paramMsg);

    std::string strParams;
    for (auto i : params) {
        strParams += i + ",";
    }

    RCLCPP_INFO_STREAM(this->get_logger(), __FUNCTION__
                                               << "RobotCmdNode sendParamMsg, cmdname: "
                                               << cmdname
                                               << "  params: "
                                               << strParams);
}

void RobotCmdNode::jsonba_callback(
    const jsonba_msg::msg::JsonBa::SharedPtr msg) const
{
    if (msg->name == "tcp") {
        unsigned int dwSize = msg->size;
        m_cmdServer->call_processTcp(dwSize);
    }
}

void RobotCmdNode::maptext_callback(
    const maptext_msg::msg::MapText::SharedPtr msg) const
{
    std::string strName = msg->name;
    if ("onGenRReady" == strName) {
        int nSize = msg->size;
        QByteArray baData;
        baData.resize(nSize);
        memcpy(baData.data(), &msg->data, nSize);
        m_cmdServer->call_onGenRReady(baData);
    } else if ("operate" == strName) {
        int nSize = msg->size;
        QByteArray baData;
        baData.resize(nSize);
        memcpy(baData.data(), &msg->data, nSize);
        m_cmdServer->call_onGenRReady(baData);
    }
}
